// Interrupts are generated every 20ms because
// Roll pin from transmitter triggers interrupt (50MHz, PWM)
// Modulo division by 5 causes each time slot to happen every 100ms

void interruptHandler() {
  switch ((timeSlot += 1) % 5) {
  case 0:
    // Read Receiver
    // This has been  tested with AR6100 and AR6200 Spektrum receivers
    roll = pulseIn(ROLLPIN, HIGH, TIMEOUT);
    aux = pulseIn(AUXPIN, HIGH, TIMEOUT);
    gear = pulseIn(GEARPIN, HIGH, TIMEOUT);
    pitch = pulseIn(PITCHPIN, HIGH, TIMEOUT);
    throttle = pulseIn(THROTTLEPIN, HIGH, TIMEOUT);
    yaw = pulseIn(YAWPIN, HIGH, TIMEOUT);
    break;
  case 2:
    // Send Data
    switch (queryType) {
    case 'X': // send no debug messages
      break;
    case 'A': // send all data
      Serial.print(deltaTime);
      comma();
      Serial.print(rollRate);
      comma();
      Serial.print(pitchRate);
      comma();
      Serial.print(yawRate);
      comma();
      Serial.print(throttle);
      comma();
      Serial.print(rollMotorCommand);
      comma();
      Serial.print(pitchMotorCommand);
      comma();
      Serial.print(yawMotorCommand);
      comma();
      Serial.print(frontCommand);
      comma();
      Serial.print(rearCommand);
      comma();
      Serial.print(rightCommand);
      comma();
      Serial.println(leftCommand);
      break;
    case 'G': // send gyro data
      Serial.print(deltaTime);
      comma();
      Serial.print(rollRate);
      comma();
      Serial.print(pitchRate);
      comma();
      Serial.println(yawRate);
      break;
    case 'P': // send PID values
      Serial.print(deltaTime);
      comma();
      Serial.print(dtostrf(pgain, 1, 2, string));
      comma();
      Serial.print(dtostrf(igain, 1, 2, string));
      comma();
      Serial.println(dtostrf(dgain, 1, 2, string));
      queryType = 'X';
      break;
    case 'R': // send receiver values
      Serial.print(dtostrf(xmitFactor, 1, 2, string));
      comma();
      Serial.print(throttle);
      comma();
      Serial.print(roll);
      comma();
      Serial.print(pitch);
      comma();
      Serial.print(yaw);
      comma();
      Serial.print(gear);
      comma();
      Serial.println(aux);
      break;
    case 'S': // read sensor values
      Serial.print(analogRead(ROLLRATEPIN));
      comma();
      Serial.print(analogRead(PITCHRATEPIN));
      comma();
      Serial.print(analogRead(YAWRATEPIN));
      comma();
      Serial.print(analogRead(2));
      comma();
      Serial.print(analogRead(3));
      comma();
      Serial.println(analogRead(4));
      break;
    case 'T': // read processed transmitter values
      Serial.print(rollCommand);
      comma();
      Serial.print(pitchCommand);
      comma();
      Serial.println(yawCommand);
      break;
    }
  default:
  break;
  }
}

void comma() {
  Serial.print(',');
}
